// 电机源文件

/*
 * 作者：勤为本
 * 勤为本的小店：https://shop333183734.taobao.com/
 */


#include <stdio.h>
#include <types.h>
#include <stdlib.h>
#include <math.h>
#include "../lib/ls1c_public.h"
#include "../lib/ls1c_pin.h"
#include "../lib/ls1c_gpio.h"
#include "../lib/ls1c_irq.h"
#include "../lib/ls1c_delay.h"
#include "../lib/ls1c_pwm.h"
#include "motor.h"


// 电机相关引脚
#define MOTOR_PWMA_PIN 				(LS1C_PWM2_GPIO46)	// pwm2/gpio46
#define MOTOR_AIN1_PIN				(58)	// GPIO58
#define MOTOR_AIN2_PIN 				(59)	// gpio59
#define MOTOR_LEFT_ENCODER_A_PIN 	(62)	// gpio62
#define MOTOR_LEFT_ENCODER_B_PIN 	(63)	// gpio63

#define MOTOR_PWMB_PIN 				(LS1C_PWM3_GPIO47)	// pwm3/gpio47
#define MOTOR_BIN1_PIN 				(60)	// gpio60
#define MOTOR_BIN2_PIN 				(61)	// gpio61
#define MOTOR_RIGHT_ENCODER_A_PIN	(64)	// gpio64
#define MOTOR_RIGHT_ENCODER_B_PIN 	(65)	// gpio65


#define TEST_PIN			(87)	// GPIO87


// 跳过死区电压
// 学习套餐的电机，在12v，占空比0.05时几乎不转，占空比0.1时，空载时可以转动，
// 所以把死区电压的占空比设为0.05
#define MOTOR_PWM_HIGH_MIN 			(MOTOR_PWM_PERIOD * 0.05)	// 一个pwm周期内高电平的最小宽度（单位，系统时钟个数）
#define MOTOR_JUMP_DEAD_ZONE(x) 	\
	if (0 > (x)) \
	{ (x) -= (-1 * MOTOR_PWM_HIGH_MIN); } \
	else { (x) += MOTOR_PWM_HIGH_MIN; }

// 限幅
#define MOTOR_PWM_HIGH_MAX 			(MOTOR_PWM_PERIOD * 0.8)	// 一个pwm周期内高电平的最大宽度（单位，系统时钟个数）
#define MOTOR_PWM_HIGH_LIMIT(x) 	\
	if (MOTOR_PWM_HIGH_MAX < (x)) \
	{ (x) = MOTOR_PWM_HIGH_MAX; } \
	else if (-1 * MOTOR_PWM_HIGH_MAX > (x)) \
	{ (x) = - 1 * MOTOR_PWM_HIGH_MAX; }

// 调速的pwm
pwm_info_t motor_a_pwm_info = {0};	// 左边电机
pwm_info_t motor_b_pwm_info = {0};	// 右边电机


// 编码器的脉冲计数器
volatile unsigned int motor_left_encoder_a_counter = 0;
volatile unsigned int motor_left_encoder_b_counter = 0;
volatile unsigned int motor_right_encoder_a_counter = 0;
volatile unsigned int motor_right_encoder_b_counter = 0;

// 最近一次读取到的编码器的脉冲数
volatile unsigned int motor_left_encoder_a_current = 0;
volatile unsigned int motor_left_encoder_b_current = 0;
volatile unsigned int motor_right_encoder_a_current = 0;
volatile unsigned int motor_right_encoder_b_current = 0;

/*
 * 电机的运转方向
 * 在编码器的A中断后，读取B的电平，来判断电机运转方向这种方案，在速度比较慢时（比如占空比0.1时），误判率很大
 * 有时误判率甚至可以达到50%，据此不采用这种方案来判断电机运转方向
 * 
 * 采用另外一种方案：从上一次电机的控制信息中获取电机方向。
 * 即在每次mpu6050读取姿态数据后，控制电机时，会设置电机运转方向。
 * 把这个方向作为其后一段时间的电机运转方向
 */
typedef enum
{
	MOTOR_DIRECTION_FORWARD = 1,			// 前进
	MOTOR_DIRECTION_BACKWARD = -1,			// 后退
}motor_direction_t;
volatile motor_direction_t motor_left_direction = 0;
volatile motor_direction_t motor_right_direction = 0;

// 电机的速度
volatile int motor_left_speed = 0;
volatile int motor_right_speed = 0;

/*
 * 左边电机的编码器A的中断处理函数
 * @IRQn 中断号
 * @param 传递给中断处理函数的参数
 */
void motor_left_encoder_a_irqhandler(int IRQn, void *param)
{
	// 计数器加一
	motor_left_encoder_a_counter++;
/*
	// 输出一个脉冲，测试用
	gpio_set(TEST_PIN, gpio_level_high);
	delay_us(10);
	gpio_set(TEST_PIN, gpio_level_low);
*/
}


/*
 * 左边电机的编码器B的中断处理函数
 * @INQn 中断号
 * @param 传递给中断处理函数的参数
 */
void motor_left_encoder_b_irqhandler(int IRQn, void *param)
{
	motor_left_encoder_b_counter++;
}


/*
 * 右边电机的编码器A的中断处理函数
 * @IRQn 中断号
 * @param 传递给中断处理函数的参数
 */
void motor_right_encoder_a_irqhandler(int IRQn, void *param)
{
	// 计数器加一
	motor_right_encoder_a_counter++;
}


/*
 * 右边电机的编码器B的中断处理函数
 * @IRQn 中断号
 * @param 传递给中断处理函数的参数
 */
void motor_right_encoder_b_irqhandler(int IRQn, void *param)
{
	motor_right_encoder_b_counter++;
}


// 电机初始化
void motor_init(void)
{
	int left_encoder_a_irq 		= LS1C_GPIO_TO_IRQ(MOTOR_LEFT_ENCODER_A_PIN);
//	int left_encoder_b_irq 		= LS1C_GPIO_TO_IRQ(MOTOR_LEFT_ENCODER_B_PIN);
	int right_encoder_a_irq 	= LS1C_GPIO_TO_IRQ(MOTOR_RIGHT_ENCODER_A_PIN);
//	int right_encoder_b_irq 	= LS1C_GPIO_TO_IRQ(MOTOR_RIGHT_ENCODER_B_PIN);
	
	// 电机驱动芯片tb6612fng初始化
	// 左边电机初始化
	gpio_init(MOTOR_AIN1_PIN, gpio_mode_output);
	gpio_set(MOTOR_AIN1_PIN, gpio_level_low);	// AIN1=0
	gpio_init(MOTOR_AIN2_PIN, gpio_mode_output);
	gpio_set(MOTOR_AIN2_PIN, gpio_level_low);	// AIN2=0
	motor_a_pwm_info.gpio	= MOTOR_PWMA_PIN;
	motor_a_pwm_info.mode 	= PWM_MODE_NORMAL;
	motor_a_pwm_info.duty 	= 0.1;
	motor_a_pwm_info.period_ns 	= 100*1000;		// pwm周期100us, 频率为10khz
	pwm_init(&motor_a_pwm_info);

	// 右边电机初始化
	gpio_init(MOTOR_BIN1_PIN, gpio_mode_output);
	gpio_set(MOTOR_BIN1_PIN, gpio_level_low);	// BIN1=0
	gpio_init(MOTOR_BIN2_PIN, gpio_mode_output);
	gpio_set(MOTOR_BIN2_PIN, gpio_level_low);	// BIN2=0
	motor_b_pwm_info.gpio 	= MOTOR_PWMB_PIN;
	motor_b_pwm_info.mode	= PWM_MODE_NORMAL;
	motor_b_pwm_info.duty	= 0.1;
	motor_b_pwm_info.period_ns	= 100*1000;		// pwm周期为100us，频率为10khz
	pwm_init(&motor_b_pwm_info);

	/*
	 * 由于龙芯1C300B CPU本身的bug，虽然把GPIO设置为下降沿触发，但实际测试的结果时上升沿和下降沿都触发
	 * 既然GPIO的上升沿和下降沿都触发，那么就没必要把编码器的A和B同时都设为中断，只需要一个即可
	 * 决定只把A设为中断，B则作为普通GPIO，读取B的高低电平判断方向即可
	 */

	// 编码器初始化
	// 左边的编码器A
	gpio_init(MOTOR_LEFT_ENCODER_A_PIN, gpio_mode_input);
	gpio_set_irq_type(MOTOR_LEFT_ENCODER_A_PIN, IRQ_TYPE_EDGE_FALLING);
	irq_install(left_encoder_a_irq, motor_left_encoder_a_irqhandler, NULL);
	irq_enable(left_encoder_a_irq);
	
	// 左边的编码器B
	gpio_init(MOTOR_LEFT_ENCODER_B_PIN, gpio_mode_input);
//	gpio_set_irq_type(MOTOR_LEFT_ENCODER_B_PIN, IRQ_TYPE_EDGE_FALLING);
//	irq_install(left_encoder_b_irq, motor_left_encoder_b_irqhandler, NULL);
//	irq_enable(left_encoder_b_irq);

	// 右边的编码器A
	gpio_init(MOTOR_RIGHT_ENCODER_A_PIN, gpio_mode_input);
	gpio_set_irq_type(MOTOR_RIGHT_ENCODER_A_PIN, IRQ_TYPE_EDGE_FALLING);
	irq_install(right_encoder_a_irq, motor_right_encoder_a_irqhandler, NULL);
	irq_enable(right_encoder_a_irq);

	// 右边的编码器B
	gpio_init(MOTOR_RIGHT_ENCODER_B_PIN, gpio_mode_input);
//	gpio_set_irq_type(MOTOR_RIGHT_ENCODER_B_PIN, IRQ_TYPE_EDGE_FALLING);
//	irq_install(right_encoder_b_irq, motor_right_encoder_b_irqhandler, NULL);
//	irq_enable(right_encoder_b_irq);

	// 初始化测试引脚，用于测试中断执行时间
	gpio_init(TEST_PIN, gpio_mode_output);
	gpio_set(TEST_PIN, gpio_level_low);

	printf("[%s] pwm period=%lu\r\n", __FUNCTION__, motor_a_pwm_info.period);
//	motor_test();

	return ;
}


/*
 * 根据占空比控制电机
 * @angle 小车倾斜的角度
 * @high_left 左边电机高电平的宽度（单位，系统时钟个数)
 * @high_right 右边电机高电平的宽度（单位，系统时钟个数）
 */
void motor_control(float angle, int high_left, int high_right)
{
	// 如果倾角太大，则关闭电机
	if ((40 < angle) || (-40 > angle))
	{
		gpio_set(MOTOR_AIN1_PIN, gpio_level_low);	// 关闭左边电机
		gpio_set(MOTOR_AIN2_PIN, gpio_level_low);
		gpio_set(MOTOR_BIN1_PIN, gpio_level_low);	// 关闭右边电机
		gpio_set(MOTOR_BIN2_PIN, gpio_level_low);
		return ;
	}

	// 跳过死区电压
	MOTOR_JUMP_DEAD_ZONE(high_left);
	MOTOR_JUMP_DEAD_ZONE(high_right);

	// 限幅
	MOTOR_PWM_HIGH_LIMIT(high_left);
	MOTOR_PWM_HIGH_LIMIT(high_right);

	// 设置电机运转方向
	if (0 < high_left)	// 左边电机
	{
		gpio_set(MOTOR_AIN1_PIN, gpio_level_low);
		gpio_set(MOTOR_AIN2_PIN, gpio_level_high);
		motor_left_direction = MOTOR_DIRECTION_BACKWARD;	// 后退
	}
	else
	{
		gpio_set(MOTOR_AIN1_PIN, gpio_level_high);
		gpio_set(MOTOR_AIN2_PIN, gpio_level_low);
		motor_left_direction = MOTOR_DIRECTION_FORWARD;		// 前进
	}
	
	if (0 < high_right) // 右边电机
	{
		gpio_set(MOTOR_BIN1_PIN, gpio_level_high);
		gpio_set(MOTOR_BIN2_PIN, gpio_level_low);
		motor_right_direction = MOTOR_DIRECTION_BACKWARD;	// 后退
	}
	else
	{
		gpio_set(MOTOR_BIN1_PIN, gpio_level_low);
		gpio_set(MOTOR_BIN2_PIN, gpio_level_high);
		motor_right_direction = MOTOR_DIRECTION_FORWARD;	// 前进
	}

	// 设置占空比
	pwm_modify_duty(motor_a_pwm_info.reg_base, 
					motor_a_pwm_info.period,
					abs(high_left));
	pwm_modify_duty(motor_b_pwm_info.reg_base,
					motor_b_pwm_info.period,
					abs(high_right));

	return ;
}


/*
 * 读取编码器的值
 */
void motor_encoder_get_speed(void)
{
	int left_speed = 0;
	int right_speed = 0;

	// 读取当前值
	motor_left_encoder_a_current = motor_left_encoder_a_counter;
	motor_left_encoder_a_counter = 0;	// 清零
	motor_left_encoder_b_current = motor_left_encoder_b_counter;
	motor_left_encoder_b_counter = 0;	// 清零
	motor_right_encoder_a_current = motor_right_encoder_a_counter;
	motor_right_encoder_a_counter = 0;
	motor_right_encoder_b_current = motor_right_encoder_b_counter;
	motor_right_encoder_b_counter = 0;

	// 速度 = 编码器a + 编码器b
	left_speed 	= motor_left_encoder_a_current + motor_left_encoder_b_current;
	right_speed	= motor_right_encoder_a_current + motor_right_encoder_b_current;

	/*
	 * 如果测得的速度过大（超过2倍最大速度），则说明此次测得的结果异常，
	 * 直接放弃本次测量结果
	 * 这种情况常见于实际速度很慢，接近于0时，可能电机在轻微的来回转动
	 */
	if ((2 * MOTOR_SPEED_MAX < left_speed) || (2 * MOTOR_SPEED_MAX < right_speed))
		return ;

	// 限幅，不能超过最大速度
	left_speed = MIN(left_speed, MOTOR_SPEED_MAX);
	right_speed = MIN(right_speed, MOTOR_SPEED_MAX);

	// 追加方向
	left_speed = motor_left_direction * left_speed;
	right_speed = motor_right_direction * right_speed;

	// 滤波
	motor_left_speed = motor_left_speed * 0.7 + left_speed * 0.3;
	motor_right_speed = motor_right_speed * 0.7 + right_speed * 0.3;
	
	return ;
}


// 电机测试函数
void motor_test(void)
{
	// 电机正转
	pwm_modify_duty(motor_a_pwm_info.reg_base,
					motor_a_pwm_info.period,
					0.1 * motor_a_pwm_info.period);
	gpio_set(MOTOR_AIN1_PIN, gpio_level_high);
	gpio_set(MOTOR_AIN2_PIN, gpio_level_low);

	pwm_modify_duty(motor_b_pwm_info.reg_base,
					motor_b_pwm_info.period,
					0.1 * motor_b_pwm_info.period);
	gpio_set(MOTOR_BIN1_PIN, gpio_level_low);
	gpio_set(MOTOR_BIN2_PIN, gpio_level_high);

	motor_left_direction = MOTOR_DIRECTION_FORWARD;
	motor_right_direction = MOTOR_DIRECTION_FORWARD;
/*
	delay_s(1);

	// 打印编码器的值
  	printf("[%s] motor_left_encoder_a_current=%u, motor_left_encoder_b_current=%u\r\n\
  	          motor_right_encoder_a_current=%u, motor_right_encoder_b_current=%u\r\n", 
			__FUNCTION__, 
			motor_left_encoder_a_current,
			motor_left_encoder_b_current,
			motor_right_encoder_a_current,
			motor_right_encoder_b_current);

	// 电机停止
	gpio_set(MOTOR_AIN1_PIN, gpio_level_low);
	gpio_set(MOTOR_AIN2_PIN, gpio_level_low);
	gpio_set(MOTOR_BIN1_PIN, gpio_level_low);
	gpio_set(MOTOR_BIN2_PIN, gpio_level_low);
	delay_s(1);

	// 电机反转
	pwm_modify_duty(motor_a_pwm_info.reg_base,
					motor_a_pwm_info.period,
					0.7 * motor_a_pwm_info.period);
	gpio_set(MOTOR_AIN1_PIN, gpio_level_high);
	gpio_set(MOTOR_AIN2_PIN, gpio_level_low);

	pwm_modify_duty(motor_b_pwm_info.reg_base,
					motor_b_pwm_info.period,
					0.7 * motor_b_pwm_info.period);
	gpio_set(MOTOR_BIN1_PIN, gpio_level_high);
	gpio_set(MOTOR_BIN2_PIN, gpio_level_low);

	delay_s(1);

	// 打印编码器的值
  	printf("[%s] motor_left_encoder_a_current=%u, motor_left_encoder_b_current=%u\r\n\
  	          motor_right_encoder_a_current=%u, motor_right_encoder_b_current=%u\r\n", 
			__FUNCTION__, 
			motor_left_encoder_a_current,
			motor_left_encoder_b_current,
			motor_right_encoder_a_current,
			motor_right_encoder_b_current);
*/
	return ;
}

